#include "board.h"


PID gyro_pid;


//角度pid初始化
void gyro_pid_init(void)
{
    pid_init(&gyro_pid, 40, 0, 35, 0, 4999,0);
}

PID* get_gyro_pid(void)
{
	return &gyro_pid;
}

int get_gyro_pid_target(void)
{
	return gyro_pid.target;
}

int gyro_Set_PWM(int gyro_initial_motor_speed, float pwm_value)
{
	if( get_angle() >= -0.5 && get_angle() <= 0.5 )
	{
		stop_motor();
		return 1;
	}
	//基础速度+PID值（基础速度为200时则定点转动）
	int left_motor_speed = gyro_initial_motor_speed - pwm_value;
	int right_motor_speed = gyro_initial_motor_speed + pwm_value;
	set_all_motor(left_motor_speed,right_motor_speed);
	return 0;
}

int gyro_control(int gyro_initial_motor_speed, float target_angle)
{
	return gyro_Set_PWM(gyro_initial_motor_speed, pid_gyro_calc(&gyro_pid,target_angle, get_angle()));
}

